/************************************************************
                SmartCar12th
      (c) Copyright 2017 HUST RENESAS LAB,The Department of CSE, HUST
                          All Rights Reserved
  Filename                :   SmartCar12th.ino
  Programmer(s)           :   Dong Zhaorui
  Description             :
  Modification History    :
  01a 2017-10-3 20:34:57
****************************************************************/
#include <Servo.h>

/*************************************************************
 ************************引脚宏定义***************************
*************************************************************/
//Pin of Sensor
//analog
////传感器输入端口定义，D7对应用于 发车 的传感器
#define  D1   A0
#define  D2   A1
#define  D3   A2
#define  D4   A3
#define  D5   A4
//digital
#define  D6   2
#define  D7   3
#define  D8   10
#define  D9   11
#define  D10  12
#define  D11  13
#define  D12  4

//Pin of Beep
//digital
#define BEEP_PIN 0

//Pin of Servo
//PWM
#define SERVO        9  //舵机PWM引脚定义

//Pin of Motor
//PWM
#define MOTOR_LEFT   5  //左电机PWM引脚定义
#define MOTOR_RIGHT  6  //右电机PWM引脚定义
//digital
#define DIR_LEFT     7  //左电机方向引脚定义
#define DIR_RIGHT    8  //右电机方向引脚定义

/*************************************************************
 **************************宏定义*****************************
*************************************************************/
//About Sensor
#define Threshold     700 //定义一个阈值
#define Sensor_1      0
#define Sensor_2      1
#define Sensor_3      2
#define Sensor_4      3
#define Sensor_5      4
#define Sensor_6      5
#define Sensor_7      6
#define Sensor_8      7
#define Sensor_9      8
#define Sensor_10     9
#define Sensor_11     10
#define Sensor_center 11

//About Beep
#define BEEP_ON       digitalWrite(BEEP_PIN, LOW)
#define BEEP_OFF      digitalWrite(BEEP_PIN, HIGH)

//About Servo
#define SERVO_CENTER  1480 //舵机中心值
#define MAX_ANGLE     450
#define AngleRate     10
#define direction_P   1 //pd控制
#define direction_D   2

//About Motor
#define Table_Length       60
#define StraightSpeed      220
#define StraightSpeed_Slow 150
#define StraightSpeed_low  80
#define R600Speed          180 //弯道速度
#define R450Speed          180
//#define SlowRate           25
#define SlowMin            1500 //减速最小计数值
#define SlowMax            2700 //减速最大计数值
#define brakeRate          4    //刹车倍率，于刹车计数值做除法得到刹车时间
#define breakMin           1000 //刹车最小计数值
#define brakeMax           6000


//About Car
#define RUN_Mode      11 //正常运行模式
#define LEFT_LOST     21 //左侧传感器丢线
#define RIGHT_LOST    22 //右侧传感器丢线
#define STOP_Mode     0  //停车模式

/*************************************************************
 **********************全局变量初始化***************************
*************************************************************/
Servo myservo; //声明myservo为 Servo类的一个实例，在这之后就可以通过myservo调用Servo类的函数

int Mode = 0;//运行模式

int Sensor_Info = 0; //一个十六位数据，用来保存传感器状态
int pre_Sensor_Info = 0; //上一次传感器状态
byte Sensor_Num  = 0; //传感器灯亮的数目

int SmartCar_Angle = 0;
int SmartCar_Speed = 0;

const int SensorDistance[11] = { -450, -400, -260, -180, -50, 0, 50, 180, 260, 400, 450}; //传感器每个红外管对应的偏差角度
/*
  const int SpeedDifference[Table_Length]  = {100, 98, 97, 95, 93, 92, 90, 88, 87, 85,
                                            84, 82, 81, 79, 78, 76, 75, 73, 72, 71,
                                            69, 68, 66, 65, 64, 62, 61, 59, 58, 57,
                                            55, 54, 52, 51, 50, 48, 47, 45, 44, 42,
                                            41, 39, 38, 36, 35, 33, 32, 30, 29, 27,
                                            25, 24, 22, 20, 19, 17, 15, 13, 11, 9
                                           };
*/
const int SpeedDifference[Table_Length]  = { 100, 98, 97, 95, 93, 91, 90, 88, 87, 85,
                                             84, 82, 80, 79, 78, 76, 75, 73, 72, 70,
                                             69, 67, 66, 65, 63, 62, 60, 59, 57, 56,
                                             55, 53, 52, 50, 49, 48, 46, 45, 43, 42,
                                             40, 39, 37, 36, 34, 33, 31, 29, 28, 26,
                                             25, 23, 21, 19, 18, 16, 14, 12, 10, 8
                                           };//定义一个差速表


/*************************************************************
 * 程序初始化
 * 功能：将需要输出信号的引脚设置为输出模式，方向引脚给低电平
*************************************************************/
void setup()
{
  pinMode(SERVO      , OUTPUT);
  pinMode(MOTOR_LEFT , OUTPUT);
  pinMode(MOTOR_RIGHT, OUTPUT);

  pinMode(DIR_LEFT , OUTPUT);
  pinMode(DIR_RIGHT, OUTPUT);

  digitalWrite(DIR_LEFT , LOW);
  digitalWrite(DIR_RIGHT, LOW);

  pinMode(BEEP_PIN, OUTPUT);
  digitalWrite(BEEP_PIN, HIGH);

  myservo.attach(SERVO); //将SERVO引脚连接到myservo
  myservo.writeMicroseconds(SERVO_CENTER);
}

/*************************************************************
 *************************程序本体****************************
*************************************************************/
void loop()
{
  if (bitRead(Sensor_Info, Sensor_center)) //读取发车传感器的状态
  {
    Mode = STOP_Mode;
  }

  Sensor_Info = Get_Sensor(); //把传感器状态保存到这个十六位数里
  //  Sensor_Num = Get_SensorNum(Sensor_Info);

  switch (Mode)
  {
    case RUN_Mode://正常运行模式
      Control();
      if ((pre_Sensor_Info == 0x0001) && (Sensor_Info == 0x0000))
      {
        Mode = LEFT_LOST;
      }
      else if ((pre_Sensor_Info == 0x0400) && (Sensor_Info == 0x0000))
      {
        Mode = RIGHT_LOST;
      }
      break;

    case LEFT_LOST://左侧传感器丢线
      analogWrite(MOTOR_LEFT, 0);
      analogWrite(MOTOR_RIGHT, 255);
      if ((bitRead(Sensor_Info, Sensor_1)) || (bitRead(Sensor_Info, Sensor_2)))
      {
        Mode = RUN_Mode;
      }
      break;

    case RIGHT_LOST://右侧传感器丢线
      analogWrite(MOTOR_LEFT, 255);
      analogWrite(MOTOR_RIGHT, 0);
      if ((bitRead(Sensor_Info, Sensor_11)) || (bitRead(Sensor_Info, Sensor_10)))
      {
        Mode = RUN_Mode;
      }
      break;

    case STOP_Mode://停车
      SmartCar_Angle = 0;
      SmartCar_Speed = 0;
      Easy_Control(1);
      if (!bitRead(Sensor_Info, Sensor_center))
      {
        Mode = RUN_Mode;
      }
      break;
  }

  pre_Sensor_Info = Sensor_Info;
}

/***************************************************************************************************************************************************************************************
 **********************************************************************各个函数*********************************************************************************************************
 ***************************************************************************************************************************************************************************************/

/*************************************************************
 ***********************获取传感器值**************************
*************************************************************/
int Get_Sensor(void)
{
  int Info = 0;

  if (!(analogRead(D1) > Threshold)) { 
    bitSet(Info, Sensor_1);
  }
  if (!(analogRead(D2) > Threshold)) {
    bitSet(Info, Sensor_2);
  }
  if (!(analogRead(D3) > Threshold)) {
    bitSet(Info, Sensor_3);
  }
  if (!(analogRead(D4) > Threshold)) {
    bitSet(Info, Sensor_4);
  }
  if (!(analogRead(D5) > Threshold)) {
    bitSet(Info, Sensor_5);
  }
  if (!(digitalRead(D6))) {
    bitSet(Info, Sensor_6);
  }
  if (!(digitalRead(D8))) {
    bitSet(Info, Sensor_7);
  }
  if (!(digitalRead(D9))) {
    bitSet(Info, Sensor_8);
  }
  if (!(digitalRead(D10))) {
    bitSet(Info, Sensor_9);
  }
  if (!(digitalRead(D11))) {
    bitSet(Info, Sensor_10);
  }
  if (!(digitalRead(D12))) {
    bitSet(Info, Sensor_11);
  }
  if (digitalRead(D7)) {
    bitSet(Info, Sensor_center);
  }

  return Info;
}

/*************************************************************
 ***********************获取传感器数**************************
*************************************************************/
int Get_SensorNum(int Info)
{
  int i = 0;
  unsigned char sensor_counter = 0;

  for (i = 0; i < 11; i ++)
  {
    sensor_counter += bitRead(Sensor_Info, i);
  }

  return sensor_counter;
}

/*************************************************************
 **********************小车控制*******************************
*************************************************************/
void Control(void)
{
  int SmartCar_Angle_Temp = 0;
  static int straight_cnt = 0; //直道计数值
  static int Stop_Flag = 0; //刹车标志
  static byte start_flag = 1;
  static byte straight_flag = 0;

  SmartCar_Angle = GetErr();
  SmartCar_Angle_Temp = SmartCar_Angle;

  if (SmartCar_Angle_Temp < 0)
  {
    SmartCar_Angle_Temp = -SmartCar_Angle_Temp;
  }

  if (straight_flag)
  {
    straight_cnt++;

    if (straight_cnt > SlowMax)
    {
      straight_cnt = SlowMax;
    }
  }
  else
  {
    straight_cnt = 0;
  }

  if ((SmartCar_Angle_Temp >= 0) && (SmartCar_Angle_Temp < 115))
  {
    straight_flag = 1;

    if (straight_cnt < SlowMin)
    {
      SmartCar_Speed = StraightSpeed;
    }
    else if (straight_cnt < SlowMax)
    {
      SmartCar_Speed = StraightSpeed_Slow;
    }
    else
    {
      SmartCar_Speed = StraightSpeed_low;
    }
  }
  else
  {
    if (straight_flag == 1)
    {
      if (straight_cnt > breakMin)
      {
        if (straight_cnt > brakeMax)
        {
          Stop_Flag = brakeMax / (brakeRate - 1);
        }
        else
        {
          Stop_Flag = straight_cnt / brakeRate;
        }
      }
      else
      {
        Stop_Flag = (straight_cnt / brakeRate)/5*4;
      }

      if (start_flag)
      {
        Stop_Flag /= 2;
        start_flag = 0;
      }

      straight_flag = 0;
      BEEP_OFF;
    }

    if (Stop_Flag)
    {
      SmartCar_Speed = - 255;
      Stop_Flag --;
    }
    else
    {
      if (SmartCar_Angle_Temp < 210)
      {
        SmartCar_Speed = R600Speed;
      }
      else
      {
        SmartCar_Speed = R450Speed;
      }
    }
  }

  Easy_Control(straight_flag);
}

/*************************************************************
 **********************误差获取*******************************
*************************************************************/
int GetErr(void)
{
  int i = 0;
  int k = 0;
  int servo_second = 0;
  static int Last_Err = 0;

  for (i = 0; i < 11; i ++)
  {
    if (bitRead(Sensor_Info, i))
    {
      if (((SensorDistance[i] - Last_Err) < 200) && ((SensorDistance[i] - Last_Err) > -200))
      {
        servo_second += SensorDistance[i];
        k++;
      }
    }
  }

  if ((k > 0) && (k < 5))
  {
    servo_second /= k;
    Last_Err = servo_second;
  }

  return Last_Err;
}

/*************************************************************
 ***********************简单控制******************************
*************************************************************/
void Easy_Control(byte stright_flag)
{
  SetServoAngle(SmartCar_Angle, stright_flag);
  SetMotorSpeed(SmartCar_Speed, SmartCar_Angle);
}

/*************************************************************
 ***********************舵机角度设置**************************
*************************************************************/
void SetServoAngle(int error, byte stright_flag)
{
  int secend = 0;
  static byte D_flag = 0;
  static byte Bow_flag = 0;
  static int last_error = 0;
  static int small_s_flag = 0;
  
  if(stright_flag)
  {
    if(Bow_flag == 1)
    {
      small_s_flag = 0;
      Bow_flag = 0;
      D_flag =0;
    }

    small_s_flag++;
    secend = direction_P*error;
  }
  else
  {
    Bow_flag = 1;
    
    if(small_s_flag > 1500)
    {
      D_flag = 1;
    }
    
    if(D_flag)
    {
      secend = direction_P*error + direction_D*(error-last_error); //通过D控制调节偏差量
    }
    else
    {
      secend = direction_P*error; //通过P控制调节偏差
    }
  }

  last_error = error;
  
  //限制舵机最大打角
  if (secend > MAX_ANGLE)
  {
    secend = MAX_ANGLE;
  }
  else if (secend < -MAX_ANGLE)
  {
    secend = -MAX_ANGLE;
  }
  
  myservo.writeMicroseconds(SERVO_CENTER + secend);
}

/*************************************************************
 *********************电机速度设置****************************
*************************************************************/
void SetMotorSpeed(int PwmDuty, int secend)
{
  int negative_angle_flag = 0;
  int PwmDuty_Inside = 0;
  int angle = secend / AngleRate;

  if (angle < 0)
  {
    angle = - angle;
    negative_angle_flag = 1;
  }

  if (angle > (Table_Length - 1))  //防止寻址溢出
  {
    angle = Table_Length - 1;
  }

  PwmDuty_Inside = PwmDuty * SpeedDifference[angle] / 100;

  if (PwmDuty < 0)
  {
    PwmDuty += 255;
    PwmDuty_Inside += 255;

    digitalWrite(DIR_LEFT , HIGH);
    digitalWrite(DIR_RIGHT, HIGH);
  }
  else
  {
    digitalWrite(DIR_LEFT , LOW);
    digitalWrite(DIR_RIGHT, LOW);
  }

//设置电机的差速，以外侧后轮为基准，内轮乘以＜1的系数实现减速，与外轮以差速实现转弯
  if (negative_angle_flag)
  {
    analogWrite(MOTOR_RIGHT, PwmDuty);
    analogWrite(MOTOR_LEFT , PwmDuty_Inside);
  }
  else
  {
    analogWrite(MOTOR_LEFT , PwmDuty);
    analogWrite(MOTOR_RIGHT, PwmDuty_Inside);
  }
}
